/*
 * Axis.cpp
 *
 *  Created on: 31.01.2020
 *      Author: Yannick
 */

#include "Axis.h"
#include "eeprom.h"
#include "eeprom_addresses.h"
#include "Leds.h"
#include "motor_def.h"
#include "canopen_motor.h"
#include "Filters.h"
#include "string.h"
#include <stdlib.h>

#ifdef WATCH_VAR
float axisSpringTorque;
float axisDamperTorque;
#endif
static bool motorHomeFlag = false;      // 电机回零标志，只要回到零就一直为true
const float AXIS_DAMPER_RATIO = 0.1;
const biquad_constant_t filterSpeedCst[4]   = {
                                                { 20, 55 },
                                                { 60, 55 },
                                                { 120, 55 },
                                                {120, 55}
                                                };

const biquad_constant_t filterAccelCst[4]   = {
                                                { 40, 30 },
                                                { 55, 30 },
                                                { 70, 30 },
                                                {120, 55}};
const biquad_constant_t filterDamperCst = {60, 55};

AxisFlashAddrs AxisXFlashAddrs = {  ADR_AXIS1_CONFIG,
                                    ADR_AXIS1_MAX_SPEED,
                                    ADR_AXIS1_MAX_ACCEL,
                                    ADR_AXIS1_DEGREES,
                                    ADR_AXIS1_POWER,
                                    ADR_AXIS1_FXRATIO,
                                    ADR_AXIS1_FRICTION,
                                    ADR_AXIS1_DAMPER,
                                    ADR_AXIS1_INERTIA,
                                    ADR_AXIS1_IDLESPRING,
                                    ADR_AXIS1_INVERTX,
                                    ADR_AXIS1_ENDSTOP,
                                    ADR_AXIS1_ENC_RATIO,
                                    ADR_AXIS1_SPEEDACCEL_FILTER,
                                    ADR_AXIS1_HOME_POS_L16,
                                    ADR_AXIS1_HOME_POS_H16
                                };
AxisFlashAddrs AxisYFlashAddrs = {  ADR_AXIS2_CONFIG,
                                    ADR_AXIS2_MAX_SPEED,
                                    ADR_AXIS2_MAX_ACCEL,
                                    ADR_AXIS2_DEGREES,
                                    ADR_AXIS2_POWER,
                                    ADR_AXIS2_FXRATIO,
                                    ADR_AXIS2_FRICTION,
                                    ADR_AXIS2_DAMPER,
                                    ADR_AXIS2_INERTIA,
                                    ADR_AXIS2_IDLESPRING,
                                    ADR_AXIS2_INVERTX,
                                    ADR_AXIS2_ENDSTOP,
                                    ADR_AXIS2_ENC_RATIO,
                                    ADR_AXIS2_SPEEDACCEL_FILTER,
                                    ADR_AXIS2_HOME_POS_L16,
                                    ADR_AXIS2_HOME_POS_H16
                                };

const float filter_fs= 1000;            // 采样频率,等效于计算频率
const int32_t damperClip = MOTOR_MAX_POWER;

//////////////////////////////////////////////
/*
 * Sources for class choosers are defined in MotorDriver and Encoder
 */
static void AxisRestoreFlash(Axis* axis);
//static void AxisSaveFlash(Axis* axis);
//static AxisConfig AxisDecodeConfFromInt(uint16_t val);
//static uint16_t AxisEncodeConfToInt(AxisConfig conf);
static int32_t AxisUpdateIdleSpringForce(Axis* axis);
static void  AxisUpdateTorqueScaler(Axis* axis);
static void AxisSetGearRatio(Axis* axis,uint8_t numerator,uint8_t denominator);
static float AxisScaleDegreeValue(int32_t enc,uint16_t degrees);

int32_t AxisScaleEncValue(float angle, uint16_t degrees);

void Axis_Init(Axis* axis)
{
    // Create HID FFB handler. Will receive all usb messages directly
    if (axis->axisId == 'X')
    {
        axis->flashAddrs = &AxisXFlashAddrs;
    }
    else if (axis->axisId == 'Y')
    {
        axis->flashAddrs = &AxisYFlashAddrs;
    }

    FAf_Init(&axis->spdlimiterAvg , 8);
    AxisRestoreFlash(axis); // Load parameters

    axis->degreesOfRotation = axis->conf.degrees;
    if(axis->degreesOfRotation < 90)                //最小90°
    {
        axis->degreesOfRotation = 90;
    }
    AxisSetEndStopStrength(axis,axis->conf.endstop);
    int32_t homePos = ((axis->conf.homePosH16<<16)&0xFFFF0000) | (axis->conf.homePosL16&0xFFFF);
    AxisSetHome(axis,homePos);
    AxisSetFxRatio(axis,axis->conf.fxratio);        //反馈强度，百分比
    AxisSetPower(axis,axis->conf.power);            //最大力矩
    AxisUpdateTorqueScaler(axis);                   //最大力矩与反馈强度相乘得到的力矩因子。
    AxisSetMaxSpeed(axis,axis->conf.maxSpeedDegS);  //设置最大速度，超过最大速度要急停
    AxisSetInvertx(axis,axis->conf.invertx);
    AxisSetDamperStrength(axis,axis->conf.damper);  //决定阻尼的大小
    AxisSetIdleSpringStrength(axis,axis->conf.idlespring);  //处理回盘速度

    BQ_SetBiquad(&axis->speedFilter,lowpass,filterSpeedCst[0].fc/filter_fs,filterSpeedCst[0].q/100.0,0.5);
    BQ_SetBiquad(&axis->accelFilter,lowpass,filterAccelCst[0].fc/filter_fs, filterAccelCst[0].q/100.0,0.5);
    BQ_SetBiquad(&axis->damperFilter,lowpass,filterDamperCst.fc/filter_fs, filterDamperCst.q/100.0,0.5);

}


/*
 * Read parameters from flash and restore settings
 */
static void AxisRestoreFlash(Axis* axis)
{
    // read all constants
    uint16_t value = 0;
    if (!EE_ReadVariable(axis->flashAddrs->addr_config, &value))
    {
        axis->conf.enctype = ((value)&0x3f);
        axis->conf.drvtype = ((value >> 6) & 0x3f);
    }
    else
    {

    }


    if (!EE_ReadVariable(axis->flashAddrs->addr_maxSpeed, &value))
    {
        axis->conf.maxSpeedDegS = value;
    }
    else
    {

    }
//
//  if (Flash_Read(flashAddrs.maxAccel, &value)){
//      this->maxTorqueRateMS = value;
//  }else{
//      pulseErrLed();
//  }

    uint16_t deg = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_degrees, &deg))
    {
        axis->conf.degrees = deg;
    }

    uint16_t power = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_power, &power))
    {
        axis->conf.power = power;
    }

    uint16_t fxratio = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_fxratio, &fxratio))
    {
        axis->conf.fxratio = fxratio;
    }

    uint16_t friction = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_friction, &friction))
    {
        axis->conf.friction = friction;
    }

    uint16_t damper = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_damper, &damper))
    {
        axis->conf.damper = damper;
    }

    uint16_t inertia = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_inertia, &inertia))
    {
        axis->conf.inertia = inertia;
    }

    uint16_t idlespring = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_idlespring, &idlespring))
    {
        axis->conf.idlespring = idlespring;
    }

    uint16_t invertx = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_invertx, &invertx))
    {
        axis->conf.invertx = invertx;
    }

    uint16_t endstop = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_endstop, &endstop))
    {
        axis->conf.endstop = endstop;
    }

    uint16_t encoderRatio = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_encoderRatio, &encoderRatio))
    {
        axis->conf.encoderRatio = encoderRatio;
    }

    uint16_t speedAccelFilter = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_speedAccelFilter, &speedAccelFilter))
    {
        axis->conf.speedAccelFilter = speedAccelFilter;
    }

    int16_t homePosL = 0,homePosH = 0;
    if(!EE_ReadVariable(axis->flashAddrs->addr_homePosL16, (uint16_t*)&homePosL) &&
       !EE_ReadVariable(axis->flashAddrs->addr_homePosH16, (uint16_t*)&homePosH))
    {
        axis->conf.homePosL16 = homePosL;
        axis->conf.homePosH16 = homePosH;

    }

}

/*
 * Called from FFBWheel->Update() via AxesManager->Update()
 */
void AxisPrepareForUpdate(Axis* axis)
{

    // Scale encoder value to set rotation range
    // Update a change of range only when new range is within valid range
    // if degree change, compute the SpeedScaler, it depends on degreesOfRotation
    if (axis->nextDegreesOfRotation != axis->degreesOfRotation && \
        abs((int)AxisScaleDegreeValue(axis->metric.current.posScale, axis->nextDegreesOfRotation)) < axis->nextDegreesOfRotation/2)
    {
        axis->degreesOfRotation = axis->nextDegreesOfRotation;
    }
}

#ifdef WATCH_VAR
int32_t endStopTorque;
#endif
/**
 * Calculate soft endstop effect 超出设定的角度之后需要加的力
 */
int16_t AxisUpdateEndstop(Axis* axis)
{
    int32_t outlevel = 0;

    if(axis->metric.current.posScale < -ET_MAX_POSTION)
    {
        //游戏方向超过负的范围的时候，需要朝顺时针加力，此时需要给伺服负方向力
        outlevel = axis->metric.current.posScale + ET_MAX_POSTION;

    }
    else if(axis->metric.current.posScale > ET_MAX_POSTION)
    {
        outlevel = axis->metric.current.posScale - ET_MAX_POSTION;
    }
    else
    {
        return 0;
    }

    float addtorque = (float)outlevel*axis->endstopStrength*ENDSTOP_RATIO;// * axis->torqueScaler; // Apply endstop gain for stiffness.
#ifdef WATCH_VAR
    endStopTorque = addtorque;
#endif
    CLIP(addtorque,-MOTOR_MAX_POWER,MOTOR_MAX_POWER);

    return addtorque;
}

/**
 * pass in ptr to receive the sum of the effects + endstop torque
 * return true if torque is clipping
**/
bool AxisUpdateTorque(Axis* axis,int32_t* totalTorque) 
{

//  if(abs(axis->effectTorque) >= ET_MAX_POWER)
//  {
//      pulseClipLed();
//  }

    // Scale effect torque

    if(axis->invertAxis)
    {
        axis->effectTorque  *= -axis->et_torqueScaler;
    }
    else
    {
        axis->effectTorque  *= axis->et_torqueScaler;
    }

    int32_t torque = axis->effectTorque + AxisUpdateEndstop(axis);  // 下发的特效
    torque += axis->axisEffectTorque * axis->torqueScaler;          // 轴特效

    // TODO超速处理,超速之后比较危险，急停？力矩设为0？
    if(axis->metric.current.speed > axis->maxSpeed)
    {

    }
    // Torque calculated. Now sending to driver
    axis->metric.current.torque = torque;

    if(motorHomeFlag)
    {
        CLIP(axis->metric.current.torque,-axis->power,axis->power);
    }
    else
    {
        CLIP(axis->metric.current.torque,-SECURITY_TORQUE,SECURITY_TORQUE);
    }


    bool torqueChanged = axis->metric.current.torque != axis->metric.previous.torque;

//  if (abs(axis->metric.current.torque) == axis->power)
//  {
//      pulseClipLed();
//  }

    *totalTorque = axis->metric.current.torque;
    return (torqueChanged);
}

void AxisUpdateDriveTorque(Axis* axis)
{
    // totalTorque = effectTorque + endstopTorque

    int32_t totalTorque=0;
    bool torqueChanged = AxisUpdateTorque(axis,&totalTorque);
    if (torqueChanged)
    {
        axis->metric.previous.torque = axis->metric.current.torque;
        // Send to motor driver
        if(totalTorque > 3){
            totalTorque += TORQUE_DEAD_BAND;
        }else if(totalTorque < -3){
            totalTorque -= TORQUE_DEAD_BAND;
        }else{      // 如果从0突然增大会造成抖动，则把else去掉
            totalTorque = 0;
        }
        Canopen_Motor_Set_TargetTorque(CANOPEN_MOTOR_1_NODEID,totalTorque,FFB_SPEED_MAX);
    }
}

/**
 * Changes the internal gearRatio scaler
 * Encoder angle is multiplied with (numerator+1)/(denominator+1)
 */
void AxisSetGearRatio(Axis* axis,uint8_t numerator,uint8_t denominator)
{
    axis->gearRatio.denominator = denominator;
    axis->gearRatio.numerator = numerator;
    axis->gearRatio.gearRatio = ((float)numerator+1.0)/((float)denominator+1.0);
}



/**
 * Returns a scaled encoder value between -0x7fff and 0x7fff with a range of degrees
 * Takes an encoder angle in degrees
 */

int32_t AxisScaleEncValue(float angle, uint16_t degrees)
{
    if (degrees == 0)
    {
        return 0;
    }

    int32_t val = (0xffff / (float)degrees) * angle;

    return val;
}


void AxisEmergencyStop(Axis* axis,bool set)
{
    Canopen_Motor_Set_TargetTorque(CANOPEN_MOTOR_1_NODEID,0,0);      // Send 0 torque first
//    Motor_Emergency_Stop(set);
    axis->control.emergency = set;
}

//void Axis::usbSuspend(){
//  if (drv != nullptr){
//      drv->turn(0);
//      drv->stopMotor();
//  }
//}

//void Axis::usbResume(){
//  if (drv != nullptr){
//      drv->startMotor();
//  }
//}
    /**           |   (rampup is from 0..5% of max degDiff)
     *            |   __________ (after use idlespringclip)
     *            |  /
     *            | /
     *            |-
     * ------------------------  degDiff
     *           -|
     *          / |
     *         /  |
     *  -------   |
     *            |
     */

static int32_t AxisUpdateIdleSpringForce(Axis* axis) 
{
    int32_t degDiff = 0;
    if(axis->metric.current.posDegrees > axis->idlespringdeadband)
    {
        degDiff = axis->metric.current.posDegrees - axis->idlespringdeadband;
    }
    else if(axis->metric.current.posDegrees < -axis->idlespringdeadband)
    {
        degDiff = axis->metric.current.posDegrees + axis->idlespringdeadband;
    }
    int32_t idleSpringForce = degDiff*axis->idlespringscale;
    if(!motorHomeFlag && abs(degDiff) < HOME_DEADBAND)
    {
        motorHomeFlag = true;
    }

    CLIP(idleSpringForce,-axis->idlespringclip,axis->idlespringclip);

#ifdef WATCH_VAR
    axisSpringTorque = idleSpringForce;
#endif
    return idleSpringForce;
}


/*
 * Called before HID effects are calculated
 * Should calculate always on and idle effects specific to the axis like idlespring and friction
 */
void AxisCalculateAxisEffects(Axis* axis,bool ffb_on)
{
    axis->axisEffectTorque = 0;
    if(!ffb_on)
    {
        axis->axisEffectTorque += AxisUpdateIdleSpringForce(axis);
    }

    // Always active damper
    if(axis->damperIntensity != 0)
    {
        float speedFiltered = (axis->metric.current.speed) * (float)axis->damperIntensity * AXIS_DAMPER_RATIO;
#ifdef WATCH_VAR
        axisDamperTorque = speedFiltered;
#endif
        CLIP(speedFiltered,-damperClip,damperClip);
        axis->axisEffectTorque += BQ_Process(&axis->damperFilter,speedFiltered);
    }
}

void AxisResetMetrics(Axis* axis,int32_t new_pos) 
{ 
    // pos is degrees
    memset(&axis->metric.current,0,sizeof(axis->metric.current));
    axis->metric.current.posRaw = new_pos;
    axis->metric.current.posScale = AxisScaleEncValue(new_pos, axis->degreesOfRotation);
    memset(&axis->metric.previous,0,sizeof(axis->metric.previous));
    // Reset filters
    BQ_CalcBiquad(&axis->speedFilter);
    BQ_CalcBiquad(&axis->accelFilter);
}

static float AxisScaleDegreeValue(int32_t pos,uint16_t degrees)
{
    float degree = 0;
    degree = (float)pos/0xffff*degrees;
    return degree;
}

void AxisUpdateMetrics(Axis* axis,float new_pos)
{

    int32_t adjPos = (new_pos - axis->homePos)*(-1);
//  axis->metric.current.posScale = adjPos*0xFFFF/0x20000*360/axis->degreesOfRotation;
    axis->metric.current.posScale = adjPos*180.0/axis->degreesOfRotation;       //按照上述式子将常数进行了化简
    axis->metric.current.posDegrees = AxisScaleDegreeValue(axis->metric.current.posScale ,axis->degreesOfRotation);

#ifdef USE_ACCEL
    float currentAccel = (currentSpeed - axis->metric.previous.speed)*1000.0/(float)tickDif;
    axis->metric.current.accel = BQ_Process(&axis->accelFilter,currentAccel);
#endif

    // store old value for next metric's computing
    axis->metric.previous = axis->metric.current;
}

int32_t AxisGetTorque(Axis* axis) 
{
    return axis->metric.current.torque;
}

void AxisSetEffectTorque(Axis* axis,int32_t torque) 
{
    axis->effectTorque = torque;
}

static void  AxisUpdateTorqueScaler(Axis* axis) 
{
    float effect_margin_scaler = ((float)axis->fx_ratio_i/100.0);
    axis->torqueScaler = ((float)axis->power / MOTOR_MAX_POWER) * effect_margin_scaler;
    axis->et_torqueScaler = axis->torqueScaler * MOTOR_MAX_POWER / ET_MAX_POWER;
}

float AxisGetTorqueScaler(Axis* axis)
{
    return axis->torqueScaler;
}

void AxisSetDegrees(Axis* axis,uint16_t degrees)
{
    uint16_t setDeg = degrees;
    CLIP(setDeg,MIN_DEGREES,MAX_DEGREES);
    axis->nextDegreesOfRotation = setDeg;
}

void AxisSetEndStopStrength(Axis* axis,uint16_t strength)
{
    axis->endstopStrength = strength;
}

void AxisSetMaxSpeed(Axis* axis,uint16_t maxSpeed)
{
    CLIP(maxSpeed,60,600);
    axis->maxSpeed = maxSpeed*6;        //rpm->°/s
}

void AxisSetHome(Axis* axis,int32_t pos)
{
    axis->homePos = pos;
}

void AxisSetPower(Axis* axis,uint16_t power)
{
    uint16_t setPower = power;
    CLIP(setPower,0,100);
    axis->power = setPower*MOTOR_MAX_POWER/100;
    AxisUpdateTorqueScaler(axis);
}

uint16_t AxisGetPower(Axis* axis)
{
    return axis->power;
}

void AxisSetFxRatio(Axis* axis,uint8_t val) 
{
    uint8_t setFxRatio = val;
    CLIP(setFxRatio,0,100);
    axis->fx_ratio_i = setFxRatio;
    AxisUpdateTorqueScaler(axis);
}

void AxisSetFriction(Axis* axis,uint8_t friction)
{
    uint8_t setFriction = friction;
    CLIP(setFriction,0,100);
    //TODO
    return;
}


void AxisSetDamperStrength(Axis* axis,uint8_t damper)
{
    uint8_t setDamper = damper;
    CLIP(setDamper,0,100);
    if(axis->damperIntensity == 0 && damper != 0)
        BQ_CalcBiquad(&axis->damperFilter);

    axis->damperIntensity = setDamper;
}

/*
 * Set the strength of the spring effect if FFB is disabled
 */
void AxisSetIdleSpringStrength(Axis* axis,uint8_t spring)
{
    uint8_t setIdleSpring = spring;
    CLIP(setIdleSpring,0,100);
    axis->idlespringclip = (int32_t)setIdleSpring*10;
    CLIP(axis->idlespringclip,0,MOTOR_MAX_POWER);
    axis->idlespringdeadband = 2;
    axis->idlespringscale = setIdleSpring*0.5;
}

void AxisSetInvertx(Axis* axis,bool invertx)
{
    axis->invertAxis = invertx;
}

bool AxisIsInverted(Axis* axis) 
{
    return axis->invertAxis; // TODO store in flash
}

metric_t* AxisGetMetrics(Axis* axis)
{
    return &axis->metric.current;
}




